/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package edu.wpi.first.wpilibj.templates;


import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.Dashboard;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Relay;
import edu.wpi.first.wpilibj.SimpleRobot;
import edu.wpi.first.wpilibj.camera.AxisCamera;
import edu.wpi.first.wpilibj.image.CriteriaCollection;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the SimpleRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class RobotTemplate extends SimpleRobot 
{
    
    //Objects
     
    //pwm ids for drive motor control
    private int frontLeftPwmId = 4; //pwm id for front left motor
    private int frontRightPwmId = 3; //pwm id for front right motor
    private int rearLeftPwmId = 2; //pwm id for rear left motor
    private int rearRightPwmId = 1; //pwm id for rear right motor
    private int shooterPwmId = 5; //pwm id for the shooter
    private int launcherPortId = 1; //launcher port id 
    
    //Control declaration
    private Joystick leftJoystick;
    private Joystick rightJoystick;
    private Joystick gamePad;
        
    //Camera declarations
    private AxisCamera camera; 
    private CriteriaCollection collection;	//criteria for analyzing image

    
    RobotDrive robotDrive; 
    Kinect autoDrive;
    
//    Shooter shooter;
    private double shooterSpeed;
    private Compressor airCompressor;
    private int shooterLowerSpeedButton = 5;
    private int shooterRaiseSpeedButton = 7;
    private int shooterFireButton = 2;
    Relay spike;
       
    //overwritten
    public void robotInit()
    {
        robotDrive = new RobotDrive(frontLeftPwmId, frontRightPwmId, rearLeftPwmId, rearRightPwmId);
        leftJoystick = new Joystick(2);
        rightJoystick = new Joystick(1);
        gamePad = new Joystick(3);
        
        airCompressor = new Compressor(1,1);
        
        shooterSpeed = 1;
        //shooter = new Shooter(shoot, shooterSpeed, launcher);
        autoDrive = new Kinect(1,2);
        
        spike = new Relay(1);
        
        
        camera = AxisCamera.getInstance("10.17.45.11");
        camera.writeResolution(AxisCamera.ResolutionT.k160x120);
        camera.writeMaxFPS(15);
        camera.writeCompression(20);
        camera.writeColorLevel(50);
        camera.writeBrightness(40);
        collection = new CriteriaCollection();
    }
    
    
    /**
     * This function is called once each time the robot enters autonomous mode.
     */
    public void autonomous() 
    {
    
        //TODO write autonomous code
    }

    /**
     * This function is called once each time the robot enters operator control.
     */
    public void operatorControl()
    {
        System.err.println("Entering teleop:");
        
        //airCompressor.start();
        spike.set(Relay.Value.kForward);
        
        while(true)
        {
            System.err.println("pressure switch value " + airCompressor.getPressureSwitchValue());
            
            if (gamePad.getRawButton(shooterLowerSpeedButton))
            {
                System.err.println("shooter speed lowered");                
                shooterSpeed = Math.min((shooterSpeed - 0.05), 0.0);
            }
            
            if (gamePad.getRawButton(shooterRaiseSpeedButton))
            {
                System.err.println("shooter speed raised");
                shooterSpeed = Math.min((shooterSpeed + 0.05), 1.0);
            }
            
            if (gamePad.getRawButton(shooterFireButton))
            {
                System.err.println("shooter activated");
            }
            
            robotDrive.tankDrive (-leftJoystick.getY(), -rightJoystick.getY());
        }
    }
    
    /**
     * This function is called once each time the robot enters test mode.
     */
    public void test() {
    
    }
}
